//
// Created by hazyparker on 2022/1/8.
// create tf data, send cmd of turtle2

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>
#include <turtlesim/Pose.h>

std::string turtle_name;

void poseCallback(const turtlesim::PoseConstPtr &msg){
    // create tf broadcaster
    static tf::TransformBroadcaster br;

    // init tf data
    tf::Transform  transform;
    transform.setOrigin(tf::Vector3(msg->x, msg->y, 0.0));
    tf::Quaternion q;
    q.setRPY(0, 0, msg->theta);
    transform.setRotation(q);

    // broadcast tf data between world and turtle(store in transform)
    // describe axis relationship of world and turtle_name
    // from now to next 10 seconds
    br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", turtle_name));
}

int main(int argc, char **argv){
    // init ros node
    ros::init(argc, argv, "my_tf_broadcaster");

    // use input param as turtle name
    if(argc != 2){
        ROS_ERROR("need turtle name as argument");
        return -1;
    }
    turtle_name = argv[1];

    // subscribe Pose topic
    ros::NodeHandle n;
    ros::Subscriber sub = n.subscribe(turtle_name+"/pose", 10, poseCallback);

    // loop, waiting for callback function
    ros::spin();

    return 0;
}

